import numpy as np
import pandas as pd

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import PoseStamped
import rclpy.parameter
import os
from rclpy.qos import QoSProfile, DurabilityPolicy


class PathPublisher(Node):
    def __init__(self):
        super().__init__("path_publisher")
        qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self.pathPub = self.create_publisher(Path, "reference_path", qos)
        self.vecPub = self.create_publisher(Float64MultiArray,"reference_velocity",qos)
        self.declare_parameter("vec",1.0)
        self.vec = self.get_parameter("vec").get_parameter_value().double_value
        self.declare_parameter("rate", -1)
        self.rate: int = self.get_parameter("rate").value
        if self.rate > 0:
            self.timer = self.create_timer(1.0 / self.rate, self.timerCallback)
        self.x = [0]
        self.y = [0]
        self.ready = False

        self.declare_parameter("path_file", "")
        path = self.get_parameter("path_file").value
        self.loadPath(path)

        if self.ready:
            self.publishPath()
            self.get_logger().info("Publish Path Once")

    def loadPath(self, path) -> tuple[list, list]:
        if path == "" or path == None or not os.path.exists(path):
            self.get_logger().error("path file not found: " + path)
            return [], []
        else:
            self.get_logger().info("load path from " + path)
        data = pd.read_csv(path)
        self.x = data["x"].tolist()
        self.y = data["y"].tolist()
        self.ready = True

    def publishPath(self):
        vec_msg = Float64MultiArray()
        for i in range(len(self.x)):
            vec_msg.data.append(self.vec)
        self.vecPub.publish(vec_msg)

        path_msg = Path()
        path_msg.header.frame_id = "map"
        path_msg.header.stamp = self.get_clock().now().to_msg()
        for i in range(len(self.x)):
            pose_stamped = PoseStamped()
            pose_stamped.header.frame_id = "map"
            pose_stamped.header.stamp = self.get_clock().now().to_msg()
            pose_stamped.pose.position.x = float(self.x[i])
            pose_stamped.pose.position.y = float(self.y[i])
            pose_stamped.pose.position.z = 0.0
            pose_stamped.pose.orientation.x = 0.0
            pose_stamped.pose.orientation.y = 0.0
            pose_stamped.pose.orientation.z = 0.0
            pose_stamped.pose.orientation.w = 1.0
            path_msg.poses.append(pose_stamped)
        self.pathPub.publish(path_msg)


    def timerCallback(self):
        if self.ready:
            self.publishPath()


def main():
    rclpy.init()
    path_publisher = PathPublisher()
    rclpy.spin(path_publisher)
    rclpy.shutdown()


if __name__ == "__main__":
    main()
